
#include "BA-3Dto2D.hpp"


using namespace Eigen;

using namespace cv;
using namespace std;


string p3d_file = "./p3d.txt"; //需要自己修改路径
string p2d_file = "./p2d.txt"; //需要自己修改路径

void bundleAdjustment (
        const vector<Point3f> points_3d,
        const vector<Point2f> points_2d,
        Mat& K );

int main(int argc, char **argv) {


    vector< Point3f > p3d;
    vector< Point2f > p2d;

    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );

    // 导入3D点和对应的2D点

    ifstream fp3d(p3d_file);
    if (!fp3d){
        cout<< "No p3d.text file" << endl;
        return -1;
    }
    else {
        while (!fp3d.eof()){
            double pt3[3] = {0};
            for (auto &p:pt3) {
                fp3d >> p;
            }
            p3d.push_back(Point3f(pt3[0],pt3[1],pt3[2]));
        }
    }
    ifstream fp2d(p2d_file);
    if (!fp2d){
        cout<< "No p2d.text file" << endl;
        return -1;
    }
    else {
        while (!fp2d.eof()){
            double pt2[2] = {0};
            for (auto &p:pt2) {
                fp2d >> p;
            }
            Point2f p2(pt2[0],pt2[1]);
            p2d.push_back(p2);
        }
    }

    assert(p3d.size() == p2d.size());

    int iterations = 100;
    double cost = 0, lastCost = 0;
    int nPoints = p3d.size();
    cout << "points: " << nPoints << endl;

    bundleAdjustment ( p3d, p2d, K );
    return 0;
}

void bundleAdjustment (
        const vector< Point3f > points_3d,
        const vector< Point2f > points_2d,
        Mat& K   )
{
    Eigen::Matrix3d KEigen;
    KEigen <<  520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
    // 创建一个线性求解器
    linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
    // 第2步：创建BlockSolver。并用上面定义的线性求解器初始化
    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
    // 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);
    // 第5步：定义图的顶点（需要补充代码）
    // 5、1加入帧位姿顶点
    int ids = 0;
    VertexPose * vFramePos = new VertexPose();
    Sophus::SE3d Twc(Eigen::Matrix4d::Identity());
    vFramePos->setEstimate(Twc);// 加入初值
    vFramePos->setId(ids++);
    vFramePos->setFixed(false);
    optimizer.addVertex(vFramePos);// 加入帧顶点

    // 5、2加入地图点顶点
    for(auto & pt : points_3d){
        Eigen::Vector3d ptEigen;
        ptEigen << pt.x, pt.y, pt.z;
        VertexPoint3* vPoint3 = new VertexPoint3();
        vPoint3->setEstimate(ptEigen);// 加入初值
        vPoint3->setId(ids++);
        vPoint3->setFixed(true);
        vPoint3->setMarginalized(true);// 设置为边缘化选项
        optimizer.addVertex(vPoint3);// 加入帧顶点
    }
    // 加入边
    vector<EdgeProjection*> edgesVec;
    for(int i = 0; i < points_3d.size(); i++){
        Eigen::Vector2d obs(points_2d[i].x, points_2d[i].y);
        EdgeProjection* e = new EdgeProjection(KEigen);
        e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(i+1)));// 2元边第0个
        e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
        e->setMeasurement(obs);
        e->setInformation(Eigen::Matrix2d::Identity());// 只有一种误差随便设置一下
        optimizer.addEdge(e);
        edgesVec.push_back(e);
    }
    // 计算所有误差
    double sumErrInit = 0.;
    for(int i = 0; i < edgesVec.size(); i++){
        edgesVec[i]->computeError();
        sumErrInit += edgesVec[i]->outError().norm();
        //std::cout << edgesVec[i]->outError() << std::endl;
    }
    // 第8步：设置优化参数，开始执行优化
    optimizer.setVerbose ( true );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );

    // 输出优化结果
    cout<<endl<<"after optimization:"<<endl;
    cout<<"T="<< endl << vFramePos->estimate().matrix() <<endl;
    // 计算所有误差
    double sumErrOut = 0.;
    for(int i = 0; i < edgesVec.size(); i++){
        edgesVec[i]->computeError();
        sumErrOut += edgesVec[i]->outError().norm();
        // std::cout << edgesVec[i]->outError() << std::endl;
    }
    std::cout << "init err = " << sumErrInit << " final err = " << sumErrOut << std::endl;
}